#include <Servo.h>

int ledPin = 13;
int Xcor = 0;
int Ycor = 0;
int AngX = 0;
int AngY = 0;
int NumberOfShots = 0;
int schieten = 8; // Pin 8 gereseveerd voor schieten
int laser = 7; // pin 7 gereseveerd voor de laser

Servo servoYas; // Servo for our Y values
Servo servoXas; // Servo for our X values


union SendPacked{
  struct test{
    int32_t Y;
    int32_t X;
    int32_t AngleX;
    int32_t AngleY;
    int32_t Fire;
  } 
  Struc;
  unsigned char bytes[20];
} 
SendData;

union RecvPacked{
  struct test{
    int32_t Y;
    int32_t X;
    int32_t AngleX;
    int32_t AngleY;
    int32_t Fire;
  } 
  Struc;
  unsigned char bytes[20];
} 
RecvData;

void Send(){
  SendData.Struc.Fire = NumberOfShots;
  SendData.Struc.X = Xcor;
  SendData.Struc.Y = Ycor;
  SendData.Struc.AngleY = AngY;
  SendData.Struc.AngleX = AngX;
  for(int i=0;i <sizeof(SendData); i++){ 
    Serial.write(SendData.bytes[i]);
  };
};

void Recieve(){
  for(int i=0;i <sizeof(RecvData); i++){ 
    RecvData.bytes[i] = Serial.read();
  };
  NumberOfShots = RecvData.Struc.Fire;
  Xcor = RecvData.Struc.X;
  Ycor = RecvData.Struc.Y;
  AngY = RecvData.Struc.AngleY;
  AngX = RecvData.Struc.AngleX;
};

void setup() {  
  pinMode(ledPin, OUTPUT); 
  pinMode(schieten, OUTPUT);
  servoYas.attach(10);      // servo on pin 10
  servoXas.attach(11);      // servo on pin 11

  Serial.begin(9600); 
  Serial.flush();

  RecvData.bytes[20];
  SendData.bytes[20];

  RecvData.Struc.Fire = 0;
  SendData.Struc.Fire = 0;

  RecvData.Struc.Y = 0;
  RecvData.Struc.X = 0;
  SendData.Struc.Y = 0;
  SendData.Struc.X = 0;
  SendData.Struc.AngleY = 0;
  SendData.Struc.AngleX = 0;
  RecvData.Struc.AngleY = 0;
  RecvData.Struc.AngleX = 0;

  // Center the servo's
  servoXas.write(90);
  servoYas.write(90);
};

void loop() {
  //while(1){
    digitalWrite(ledPin,HIGH);
    if (Serial.available() == 20) {
      Recieve();
      // Small motor code shit      
      servoXas.write(RecvData.Struc.AngleX);
      //servoXas.write(90);
      servoYas.write(RecvData.Struc.AngleY);
      
      if(NumberOfShots > 0){
        for(int i=0; i < NumberOfShots; i++){
          digitalWrite(schieten,HIGH);
          delay(5);
          digitalWrite(schieten,LOW);
        };
        
        NumberOfShots = 0;
      };
      //servoYas.write(90);
      Send();
    }
    Serial.flush();
    digitalWrite(ledPin,LOW);
  //};
}

